#pragma once
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>

using namespace std;
using namespace cv;

namespace RobotVision
{
	class RSolvePnpParam
	{
	public:
		RSolvePnpParam(vector<Point3f> objectPoints,vector<Point2f> imagePoint2D,Mat cameraMatrix,Mat distCoeffs)
			:ObjectPoint3D(objectPoints), 
			ImagePoint2D(imagePoint2D), 
			CameraMatrix(cameraMatrix),
			DistCoeffs(distCoeffs){}
		vector<Point3f> ObjectPoint3D;
		vector<Point2f> ImagePoint2D;
		Mat CameraMatrix;
		Mat DistCoeffs;
	};

	class RSolvePnpResult
	{
	public:
		Mat Tvec;
		Mat Rvec;
	};


	class RSolvePnp
	{
	public:
		void Initialize(RSolvePnpParam *model);
		void Run();
		RSolvePnpResult GetResult() { return *m_Result; };
	private:
		RSolvePnpParam *m_InParams;
		RSolvePnpResult*m_Result=new RSolvePnpResult();
		bool solvePnP();
	};
}


